# -*- coding: utf-8 -*-
import rospy
import tf
from geometry_msgs.msg import PoseStamped

class LidarPositionFetcher:
    def __init__(self):
        rospy.init_node('lidar_position_fetcher')
        self.listener = tf.TransformListener()
        rospy.sleep(1)  # 等待tf缓存填充

    def get_lidar_position(self):
        try:
            (trans, rot) = self.listener.lookupTransform('/map', '/lidar_frame', rospy.Time(0))
            print("Translation: ", trans)
            print("Rotation: ", rot)
            return trans, rot
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            rospy.logerr("TF Exception")
            return None, None

if __name__ == '__main__':
    fetcher = LidarPositionFetcher()
    rate = rospy.Rate(1)  # 1 Hz
    while not rospy.is_shutdown():
        trans, rot = fetcher.get_lidar_position()
        if trans and rot:
            print("Lidar Position: x={}, y={}, z={}".format(trans[0], trans[1], trans[2]))
            print("Lidar Orientation (quaternion): x={}, y={}, z={}, w={}".format(rot[0], rot[1], rot[2], rot[3]))
        rate.sleep()
